#include "modules/CtrlModule/PassivePlanner/SawtoothWallPlanner.h"
#include "data_struct/base/DataSet.h"
#include "lib/Time.h"

namespace behavior_controller
{

//用于判断状态与状态跳转
#define INIT_STATE 0
#define EXCEED_MOVE 1
#define ROTATION_ANGLE 2
#define FRONT_MOVE 3

    SawtoothWallPlanner sawtooth_wall_planner;
    void SawtoothWallPlanner::run(double &linear_vel, double &angular_vel)
    {
        if ((state_step == INIT_STATE) || (state_step == EXCEED_MOVE)) //判断当前状态是否为初始状态或超远墙面行走状态
        {
            state_step = EXCEED_MOVE;
            exceed_wall_front_move(linear_vel, angular_vel);
        }
        else if (state_step == ROTATION_ANGLE) //判断当前状态是否为旋转90状态
        {
            rotation_right_angle(linear_vel, angular_vel);
        }
        else if (state_step == FRONT_MOVE) //判断当前状态是否为向墙面行走状态
        {
            front_move(linear_vel, angular_vel);
        }
    }

    void SawtoothWallPlanner::exceed_wall_front_move(double &linear_vel, double &angular_vel)
    {
        linear_vel = 0.25;
        angular_vel = 0.0;
        double time_diff = ROBOT_RADIUS / linear_vel;

        if (time_now < 1e-4)
            time_now = Time::Now(); //如果时间为0,才会获取当前时间，以免下个周期到来时，时间变为最新时间
        if (Time::Now() - time_now < time_diff)
        {
        }
        else if (Time::Now() - time_now > time_diff)
        {
            time_now = 0;
            linear_vel = 0.0;
            angular_vel = 0.0;
            state_step = ROTATION_ANGLE;
        }
    }

    void SawtoothWallPlanner::rotation_right_angle(double &linear_vel, double &angular_vel)  //dpj  应为向左转
    {
        linear_vel = 0.0;
        angular_vel = 0.5; //dpj  angular_vel>0 是左转
        //double time_diff = M_PI / 2.0 / linear_vel;
        double time_diff = M_PI / 2.0 / angular_vel;   //dpj

        if (time_now < 1e-4)
            time_now = Time::Now(); //如果时间为0,才会获取当前时间，以免下个周期到来时，时间变为最新时间
        if (Time::Now() - time_now < time_diff)
        {
        }
        else if (Time::Now() - time_now > time_diff)
        {
            time_now = 0;
            linear_vel = 0.0;
            angular_vel = 0.0;
            state_step = FRONT_MOVE;
        }
    }

    void SawtoothWallPlanner::front_move(double &linear_vel, double &angular_vel)
    {
        linear_vel = 0.25;
        angular_vel = 0.0;

        if (time_now < 1e-4)
            time_now = Time::Now(); //如果时间为0,才会获取当前时间，以免下个周期到来时，时间变为最新时间
        if (Time::Now() - time_now < 1)
        {
        }
        else if (Time::Now() - time_now > 1)
        {
            time_now = 0;
            linear_vel = 0.0;
            angular_vel = 0.0;
            finished = true;
        }
    }

} // namespace behavior_controller
